package various;


import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.ColorSensor;

public class Solver {

	private static int m; // righe
	private static int n; // colonne
	private static int[][] labirinto;

	private static int entrata; // righe
	private static int uscita; // colonne

	private static int angle_x; // movimenti dx-sx
	private static int angle_y;// movimenti su-giu
	private static int cicli_int;
	private static int cicli_ex;
	private static int speed;
	private static int speed_rw;

	public static void main(String[] args) throws Exception {

		m = 12;
		n = 12;
		cicli_ex = m;// righe
		cicli_int = n;// colonne

		labirinto = new int[m][n];

		angle_y = -30;
		angle_x = 230;

		speed = 75;
		speed_rw = 750;

		Colori.initColori();
		ColorSensor cs = new ColorSensor(SensorPort.S1);

		calibra(cs);
		Thread.sleep(2000);

		// lettura labirinto

		for (int i = 0; i < cicli_ex; i++) {
			for (int j = 0; j < cicli_int; j++) {
				Motor.A.regulateSpeed(true);
				LCD.clear();
				LCD.drawString(Colori.getColore(cs.getColorNumber()) + "  i: "
						+ i + " j: " + j, 0, 1); // legge il colore
				labirinto[i][j] = cs.getColorNumber();
				if (i == 0 && labirinto[i][j] == 17)
					entrata = j;
				if (i == m - 1 && labirinto[i][j] == 17)
					uscita = j;
				Thread.sleep(500);
				Motor.A.rotate(angle_x);
				Motor.A.stop();
			}

			riavvolgiMotore(angle_x, cicli_int, MotorPort.A);
			Motor.B.regulateSpeed(true);
			Motor.B.setSpeed(speed);
			Motor.B.rotate(angle_y);
			Thread.sleep(500);
		}

		LCD.clear();
		stampaMatrice(labirinto);
		LCD.clear();
		LCD.drawString("Conversione...", 0, 1);
		convertiMatrice(labirinto);
		LCD.clear();
		LCD.drawString("Risoluzione...", 0, 1);

		if (esplora(0, entrata, m - 1, uscita)) {
			LCD.clear();
			LCD.drawString("Risolto!", 0, 1);
			riconvertiMatrice(labirinto);
			stampaMatrice(labirinto);
		}

		Thread.sleep(30000);

	}

	private static void riconvertiMatrice(int[][] labirinto) {
		for (int i = 0; i < m; i++) {
			for (int j = 0; j < n; j++) {
				if (labirinto[i][j] == 1)
					labirinto[i][j] = 0; // sostituzione di nero con 1
				else if (labirinto[i][j] == 0)
					labirinto[i][j] = 17; // sostituzione di bianco con 0
			}
		}

	}

	public static void stampaMatrice(int[][] labirinto) {
		for (int i = m - 1; i >= 0; i--) {
			for (int j = 0; j < n; j++) {
				String w = Colori.getColoreCh(labirinto[i][j]);
				LCD.drawString(w, j, i);
			}

		}

	}

	private static void convertiMatrice(int[][] labirinto) {
		for (int i = 0; i < m; i++) {
			for (int j = 0; j < n; j++) {
				if (labirinto[i][j] == 0)
					labirinto[i][j] = 1; // sostituzione di nero con 1
				else if (labirinto[i][j] == 17)
					labirinto[i][j] = 0; // sostituzione di qualsiasi colore
											// diverso da nero con 0
			}
		}

	}

	public static boolean esplora(int i, int j, int h, int k) {

		if ((i < 0) || (i >= m) || (j < 0) || (j >= n))
			return false;

		if ((h < 0) || (h >= m) || (k < 0) || (k >= n))
			return false;

		if (labirinto[i][j] != 0)
			return false;

		if ((i == h) && (j == k)) {
			labirinto[i][j] = 19; // marco come visitato
			return true; // inizio e fine coincidono
		}

		labirinto[i][j] = 19; // marco come visitato

		if (esplora(i, j + 1, h, k)) { // est
		// System.out.println(""+i+","+(j+1));
			return true;
		}

		if (esplora(i - 1, j, h, k)) { // nord
		// System.out.println(""+(i-1)+","+j);
			return true;
		}

		if (esplora(i, j - 1, h, k)) { // ovest
		// System.out.println(""+i+","+(j-1));
			return true;
		}

		if (esplora(i + 1, j, h, k)) { // sud
		// System.out.println(""+(i+1)+","+j);
			return true;
		}
		return false;

	}

	private static void riavvolgiMotore(int angle, int cicli, MotorPort port) {
		Motor motore = new Motor(port);
		motore.setSpeed(speed_rw);
		motore.rotate(-angle * cicli);
		motore.setSpeed(speed);
	}

	private static void riavvolgiUS(UltrasonicSensor us) throws Exception {
		us.getDistance();
		Thread.sleep(500);
		us.getDistance();
		while (us.getDistance() >= 5) {
			Motor.A.setSpeed(300);
			Motor.A.backward();
			LCD.clear();
			LCD.drawString("cm: " + us.getDistance(), 0, 4);
		}

	}

	public static void calibra(ColorSensor cs) throws Exception {
		Motor.A.regulateSpeed(true);
		int c = 1;
		int d = 1;
		while (c == 1 && d == 1) {
			LCD.clear();
			LCD.drawString("Calibrazione bianco...", 0, 2);
			Thread.sleep(500);
			c = cs.initWhiteBalance();
			LCD.drawString("Bianco eseguita", 0, 3);
			LCD.drawInt(c, 0, 3);
			Thread.sleep(500);

			Motor.A.rotate(angle_x);
			LCD.clear();
			LCD.drawString("Calibrazione nero...", 0, 2);
			Thread.sleep(500);
			d = cs.initBlackLevel();
			LCD.drawString("Nero eseguita", 0, 3);
			LCD.drawInt(d, 0, 6);
			Thread.sleep(500);
		}
		LCD.clear();
		Motor.A.regulateSpeed(true);
		Motor.A.rotate(-angle_x);
		Thread.sleep(500);
		Motor.B.setSpeed(speed);
		Motor.B.regulateSpeed(true);
		Motor.B.rotate(angle_y);
		Thread.sleep(500);

	}

}// classe

